import optimize,shape,B_Spline
from matplotlib import pyplot as plt
import numpy as np
import time


class Path():
    def __init__(self,border, dense, barriers):
        self.border = border
        self.dense = dense
        self.barriers = barriers
        self.path = []
        self.trace_stack = []
        self.stack_len = 8
        self.pointer = -1

    # 二分插值
    @staticmethod
    def Bisect_subdivision(num, head, tail):
        res = [head]
        for i in range(num):
            start = res[-1]
            mid = [(start[0] + tail[0])/2, (start[1] + tail[1])/2]
            res.append(mid)
        res.append(tail)
        return res

    # 夹角插值
    @staticmethod
    def Corner_subdivision(head, tail, dim = 1):
        p1 = head
        p2 = tail[1]
        p0 = tail[0]
        v1 = np.array([p1[0] - p0[0], p1[1] - p0[1]])
        v2 = np.array([p2[0] - p0[0], p2[1] - p0[1]])
        vdot = np.dot(v1 , v2)
        d1 = np.linalg.norm(v1)
        d2 = np.linalg.norm(v2)

        # 计算夹角余弦
        if d1 != 0 and d2 != 0:
            vcos = vdot / d1 / d2 + 1
        else:
            vcos = 0

        num = int(vcos * 3) - dim

        if num < 0:
            num = 0

        head_res = Path.Bisect_subdivision(num, p1, p0)[:-1]
        tail_res = Path.Bisect_subdivision(num, p2, p0)[::-1]
        return head_res + tail_res

    def Path_Subdivision(self,start,end,d = 0):
        if d == 0:
            d = shape.distance(start,end)
        thres = self.dense / 3
        if d > thres:
            divs = int(d / thres)
            path = []
            x_list = np.linspace(start[0], end[0], divs)
            y_list = np.linspace(start[1], end[1], divs)
            for i in range(divs):
                path.append([x_list[i],y_list[i]])
            return path
        else:
            return [start]
        
    def Path_Global_Search(self, start = [0,0], n = 270):
        optim = optimize.Path_Optimize(self.border, self.dense, self.barriers, start)

        try:
            path_list = optim.run()

            bs = B_Spline.BSpline(path_list)
            path_curve = bs.curve(n).tolist()
        except:
            path_curve = [[0,0],[1,1],[0,0]]

        self.path.extend(path_curve) ##
        return path_curve

    def Path_Through_Ring(self, start, end):
        Ring = self.barriers[0]

        # 生成环形障碍中线处的路径点
        path_c = Ring.get_through_path(4)
        b,bt = Ring.get_endpoint()
        p,pt = (path_c[0],path_c[-1])
        path = []
        d_sum = 0

        # 分别计算起点到中线路径点两端的距离
        ds = shape.distance(start,p)
        dsc = shape.distance(start,pt)

        # 判断先走那一端
        if ds > dsc:
            path_c = path_c[::-1]

        path.extend([start]) 
        dmin = min(ds, dsc)
        d_sum += dmin + Ring.len

        # 将过长的路径连接拆分
        sub_path = self.Path_Subdivision(start,path_c[0],dmin)[:-1]
        path.extend(sub_path)
        # 将连接处的路径点细化
        connect_path = Path.Corner_subdivision(path[-1],path_c[:2])
        path.extend(connect_path[1:])
        path.extend(path_c[2:])        

        p,pt = (path_c[0],path_c[-1])
        de = shape.distance(end,pt)
        der = shape.distance(end,p)
 
        if de <= der:
            sub_path = self.Path_Subdivision(path_c[-1],end,de)
            d_sum += de
        else:
            inv_path_c = path_c[::-1]
            connect_path = Path.Corner_subdivision(path[-1],inv_path_c[:2])
            sub_path = self.Path_Subdivision(path_c[0],end,der)
            path.extend(connect_path)
            path.extend(inv_path_c[2:])
            d_sum += der + Ring.len

        connect_path = Path.Corner_subdivision(path[-2],sub_path[:2], dim = 2)
        path = path[:-1]
        path.extend(connect_path[1:])
        path.extend(sub_path[2:])
        path.extend([end])

        # 根据距离计算适当的路径点个数          
        n = int(d_sum / 5)

        # print("***path***", path)
        bs = B_Spline.BSpline(path)
        path_curve = bs.curve(n).tolist()
        self.path.extend(path_curve)      ##
        return path_curve
    
    def Pos_Anti_Collision(self, pos, target):
        ptar = shape.Pillar([0,0], 0, 1)
        ptar.set_endpoint(pos, target)

        bpos1, bpos2 = self.barriers[0].get_endpoint()

        bpos = [bpos1, bpos2]

        new_tar_x, new_tar_y = ptar.redirect(bpos, self.barriers[0].len/2)

        if new_tar_x == -1 and new_tar_y == -1:
            return target
        else:
            return [new_tar_x, new_tar_y]


    def get_path(self):
        return self.path
    
    def draw(self, tag = "sec", path = '/home/ninetripod/catkin_ws/src/uav/log/',save = False):
        _path = np.array(self.path)
        px = _path[:, :1].flatten()
        py = _path[:, 1:].flatten()

        if tag == 'search':
            plt.clf()
            plt.title('Path generated at {}:{}'.format(time.gmtime().tm_min, time.gmtime().tm_sec))
            for sp in self.barriers:
                sp.draw()
            plt.scatter(px, py, s = 5, c = 'r')
        else:
            plt.scatter(px, py, s = 5, c = 'g')

        if save:
            if tag == "sec":
                plt.savefig('{}path_{}.png'.format(path,time.gmtime().tm_sec), dpi=300)
            else:
                plt.savefig('{}path_{}.png'.format(path,tag), dpi=300)
                # plt.show()

        

    def init_tracker(self, length = 7):
        self.pointer = -1
        self.trace_stack = []
        self.stack_len = length
        self.path = []


    def Tracker(self, pos):
        if self.pointer == -1:
            for i in range(self.stack_len):
                self.trace_stack.append(self.path[i])
            self.pointer += 1
            return self.path[0]
        else:
            tar = self.trace_stack[-1]
            cur = self.trace_stack[0]
            next = self.trace_stack[1]
            dc = shape.distance(cur,pos)
            dn = shape.distance(next,pos)
            dtar = shape.distance(tar,pos)
            if self.pointer + self.stack_len > len(self.path) - 1:
                self.pointer = -1
                return self.path[-1]
            if dc >= dn or dtar < 5:
                self.trace_stack.append(self.path[self.pointer + self.stack_len])
                self.trace_stack = self.trace_stack[1:]
                self.pointer += 1
            return tar
        
# obstacle = [shape.Pillar([100, 100], 0, 100)]
# b1, b2 = obstacle[0].get_endpoint()
# border = [-25, -25, 470, 300]
# dense = 80
# path_planner = Path(border, dense, obstacle)
# h = 90
# pos = [198,89]
# tar = [180,110]
# newpos = path_planner.Pos_Anti_Collision(pos, tar)
# print(newpos)
# plt.scatter([b1[0], b2[0]], [b1[1],b2[1]], s = 5, c = 'r')
# plt.scatter([pos[0], tar[0]], [pos[1],tar[1]], s = 5, c = 'g')
# plt.scatter([newpos[0]], [newpos[1]], s = 5, c = 'y')
# plt.scatter([0,200,200,0], [0,0,200,200], s = 5, c = 'b')
# plt.axis('equal')
# plt.show()


# import random

# for i in range(50):
#     obstacle2 = [shape.Pillar([random.random()*390 + 40, random.random()*220 + 40], random.random()*180 - 90, 110)]

#     border = [-25, -25, 470, 300]
#     dense = 80
#     path_planner = Path(border, dense, obstacle2)
#     paths = path_planner.Path_Global_Search()
#     end = paths[-2]
#     path_planner.draw('search','./res/')
#     path_planner.init_tracker()
#     print('###',end)
#     path_planner.Path_Through_Ring(end, [0.0, 0.0])
#     path_planner.draw('{}'.format(i),'./res/',True)





